package be.rivendale.ghetto;

import be.rivendale.geometry.AxisAlignedBoundingBox;
import be.rivendale.mathematics.Triangle;

public class AabbTriangleIntersectionGhetto {
	private static final int X = 0;
	private static final int Y = 1;
	private static final int Z = 2;

	public static boolean intersects(AxisAlignedBoundingBox voxel, Triangle triangle) {
		double[] halfSizeOfVoxel = {
				voxel.getWidth() / 2,
				voxel.getHeight() / 2,
				voxel.getDepth() / 2
		};

		double[] centerOfBox = {
				voxel.getMinimumBound().getX() + halfSizeOfVoxel[0],
				voxel.getMinimumBound().getY() + halfSizeOfVoxel[1],
				voxel.getMinimumBound().getZ() + halfSizeOfVoxel[2]
		};

		double[][] triangleVertices = {
				{ triangle.getA().getX(), triangle.getA().getY(), triangle.getA().getZ() },
				{ triangle.getB().getX(), triangle.getB().getY(), triangle.getB().getZ() },
				{ triangle.getC().getX(), triangle.getC().getY(), triangle.getC().getZ() }
		};

		int result = triBoxOverlap(centerOfBox, halfSizeOfVoxel, triangleVertices);
		return result == 1;
	}

	/**
	 * use separating axis theorem to test overlap between triangle and box
	 * need to test for overlap in these directions:
	 * 1) the {x,y,z}-directions (actually, since we use the AABB of the triangle
	 * we do not even need to test these)
	 * 2) normal of the triangle
	 * 3) crossproduct(edge from tri, {x,y,z}-directin)
	 * this gives 3x3=9 more tests
	 */
	private static int triBoxOverlap(double[] boxcenter, double[] boxhalfsize, double[][] triverts) {
		double[] v0 = new double[3];
		double[] v1 = new double[3];
		double[] v2 = new double[3];

		ReferenceObject ro = new ReferenceObject();

		double fex, fey, fez;
		double normal[] = new double[3];
		double e0[] = new double[3];
		double e1[] = new double[3];
		double e2[] = new double[3];

		/* This is the fastest branch on Sun */
		/* move everything so that the boxcenter is in (0,0,0) */
		SUB(v0, triverts[0], boxcenter);
		SUB(v1, triverts[1], boxcenter);
		SUB(v2, triverts[2], boxcenter);

		/* compute triangle edges */
		SUB(e0, v1, v0);      /* tri edge 0 */
		SUB(e1, v2, v1);      /* tri edge 1 */
		SUB(e2, v0, v2);      /* tri edge 2 */

		/* Bullet 3:  */
		/*  test the 9 tests first (this was faster) */
		fex = fabsf(e0[X]);
		fey = fabsf(e0[Y]);
		fez = fabsf(e0[Z]);
		if(AXISTEST_X01(e0[Z], e0[Y], fez, fey, ro, v0, v2, boxhalfsize) != null) { return 0; }
		if(AXISTEST_Y02(e0[Z], e0[X], fez, fex, ro, v0, v2, boxhalfsize) != null) { return 0; }
		if(AXISTEST_Z12(e0[Y], e0[X], fey, fex, ro, v1, v2, boxhalfsize) != null) { return 0; }

		fex = fabsf(e1[X]);
		fey = fabsf(e1[Y]);
		fez = fabsf(e1[Z]);
		if(AXISTEST_X01(e1[Z], e1[Y], fez, fey, ro, v0, v2, boxhalfsize) != null) { return 0; }
		if(AXISTEST_Y02(e1[Z], e1[X], fez, fex, ro, v0, v2, boxhalfsize) != null) { return 0; }
		if(AXISTEST_Z0(e1[Y], e1[X], fey, fex, ro, v0, v1, boxhalfsize) != null) { return 0; }

		fex = fabsf(e2[X]);
		fey = fabsf(e2[Y]);
		fez = fabsf(e2[Z]);
		if(AXISTEST_X2(e2[Z], e2[Y], fez, fey, ro, v0, v1, boxhalfsize) != null) { return 0; }
		if(AXISTEST_Y1(e2[Z], e2[X], fez, fex, ro, v0, v1, boxhalfsize) != null) { return 0; }
		if(AXISTEST_Z12(e2[Y], e2[X], fey, fex, ro, v1, v2, boxhalfsize) != null) { return 0; }

		/* Bullet 1: */
		/*  first test overlap in the {x,y,z}-directions */
		/*  find min, max of the triangle each direction, and test for overlap in */
		/*  that direction -- this is equivalent to testing a minimal AABB around */
		/*  the triangle against the AABB */
		/* test in X-direction */
		FINDMINMAX(v0[X],v1[X],v2[X], ro);
		if(ro.min>boxhalfsize[X] || ro.max<-boxhalfsize[X]) return 0;

		/* test in Y-direction */
		FINDMINMAX(v0[Y],v1[Y],v2[Y], ro);
		if(ro.min>boxhalfsize[Y] || ro.max<-boxhalfsize[Y]) return 0;

		/* test in Z-direction */
		FINDMINMAX(v0[Z],v1[Z],v2[Z], ro);
		if(ro.min>boxhalfsize[Z] || ro.max<-boxhalfsize[Z]) return 0;

		/* Bullet 2: */
		/*  test if the box intersects the plane of the triangle */
		/*  compute plane equation of triangle: normal*x+d=0 */
		CROSS(normal,e0,e1);
		// -NJMP- (line removed here)
		if(!planeBoxOverlap(normal, v0, boxhalfsize)) return 0;	// -NJMP-
		return 1;   /* box and triangle overlaps */
	}

	private static boolean planeBoxOverlap(double normal[], double vert[], double maxbox[]) {	// -NJMP-
		int q;
		double[] vmin = new double[3];
		double[] vmax = new double[3];
		double v;

		for(q = X; q <= Z; q++) {
			v = vert[q];					// -NJMP-
			if(normal[q] > 0.0) {
				vmin[q] = -maxbox[q] - v;	// -NJMP-
				vmax[q] = maxbox[q] - v;	// -NJMP-
			} else {
				vmin[q] = maxbox[q] - v;	// -NJMP-
				vmax[q] = -maxbox[q] - v;	// -NJMP-
			}
		}

		if(DOT(normal, vmin) > 0.0) {
			return false;
		}// -NJMP-
		if(DOT(normal, vmax) >= 0.0) {
			return true;
		}	// -NJMP-
		return false;
	}

	/**
		#define DOT(v1,v2) (v1[0]*v2[0]+v1[1]*v2[1]+v1[2]*v2[2])
	 */
	private static double DOT(double[] v1, double[] v2) {
		return (v1[0] * v2[0] + v1[1] * v2[1] + v1[2] * v2[2]);
	}

	/**
	 * #define AXISTEST_X01(a, b, fa, fb)			   \
	 * p0 = a*v0[Y] - b*v0[Z];			       	   \
	 * p2 = a*v2[Y] - b*v2[Z];			       	   \
     *   if(p0<p2) {min=p0; max=p2;} else {min=p2; max=p0;} \
	 * rad = fa * boxhalfsize[Y] + fb * boxhalfsize[Z];   \
	 * if(min>rad || max<-rad) return 0;
	 */
	private static Integer AXISTEST_X01(final double a, final double b, final double fa, final double fb, ReferenceObject ro, final double[] v0, final double[] v2, final double[] boxhalfsize) {
		ro.p0 = a * v0[Y] - b * v0[Z];
		ro.p2 = a * v2[Y] - b * v2[Z];
		if(ro.p0 < ro.p2) {
			ro.min = ro.p0;
			ro.max = ro.p2;
		} else {
			ro.min = ro.p2;
			ro.max = ro.p0;
		}
		ro.rad = fa * boxhalfsize[Y] + fb * boxhalfsize[Z];
		if(ro.min > ro.rad || ro.max < -ro.rad) {
			return 0;
		}
		return null;
	}

	/**
		#define AXISTEST_Y02(a, b, fa, fb)			   \
		p0 = -a*v0[X] + b*v0[Z];		      	   \
		p2 = -a*v2[X] + b*v2[Z];	       	       	   \
		if(p0<p2) {min=p0; max=p2;} else {min=p2; max=p0;} \
		rad = fa * boxhalfsize[X] + fb * boxhalfsize[Z];   \
		if(min>rad || max<-rad) return 0;
	 */
	private static Integer AXISTEST_Y02(double a, double b, double fa, double fb, ReferenceObject ro, final double[] v0, final double[] v2, final double[] boxhalfsize) {
		ro.p0 = -a * v0[X] + b * v0[Z];
		ro.p2 = -a * v2[X] + b * v2[Z];
       	if(ro.p0 < ro.p2) {
		   ro.min = ro.p0;
		   ro.max = ro.p2;
		} else {
		   ro.min = ro.p2;
		   ro.max = ro.p0;
	   	}
		ro.rad = fa * boxhalfsize[X] + fb * boxhalfsize[Z];
		if(ro.min > ro.rad || ro.max < -ro.rad) {
			return 0;
		}
		return null;
	}

	/**
		#define AXISTEST_Z12(a, b, fa, fb)			   \
		p1 = a*v1[X] - b*v1[Y];			           \
		p2 = a*v2[X] - b*v2[Y];			       	   \
			if(p2<p1) {min=p2; max=p1;} else {min=p1; max=p2;} \
		rad = fa * boxhalfsize[X] + fb * boxhalfsize[Y];   \
		if(min>rad || max<-rad) return 0;
	 */
	private static Integer AXISTEST_Z12(double a, double b, double fa, double fb, ReferenceObject ro, final double[] v1, final double[] v2, final double[] boxhalfsize) {
		ro.p1 = a*v1[X] - b*v1[Y];
		ro.p2 = a*v2[X] - b*v2[Y];
			if(ro.p2<ro.p1) {ro.min=ro.p2; ro.max=ro.p1;} else {ro.min=ro.p1; ro.max=ro.p2;}
		ro.rad = fa * boxhalfsize[X] + fb * boxhalfsize[Y];
		if(ro.min>ro.rad || ro.max<-ro.rad) {return 0; };
		return null;
	}

	/**
		#define AXISTEST_Z0(a, b, fa, fb)			   \
		p0 = a*v0[X] - b*v0[Y];				   \
		p1 = a*v1[X] - b*v1[Y];			           \
			if(p0<p1) {min=p0; max=p1;} else {min=p1; max=p0;} \
		rad = fa * boxhalfsize[X] + fb * boxhalfsize[Y];   \
		if(min>rad || max<-rad) return 0;
	 */
	private static Integer AXISTEST_Z0(double a, double b, double fa, double fb, ReferenceObject ro, double[] v0, double[] v1, double[] boxhalfsize) {
		ro.p0 = a*v0[X] - b*v0[Y];
		ro.p1 = a*v1[X] - b*v1[Y];
			if(ro.p0<ro.p1) {ro.min=ro.p0; ro.max=ro.p1;} else {ro.min=ro.p1; ro.max=ro.p0;}
		ro.rad = fa * boxhalfsize[X] + fb * boxhalfsize[Y];
		if(ro.min>ro.rad || ro.max<-ro.rad) return 0;
		return null;
	}

	/**
		#define AXISTEST_X2(a, b, fa, fb)			   \
		p0 = a*v0[Y] - b*v0[Z];			           \
		p1 = a*v1[Y] - b*v1[Z];			       	   \
			if(p0<p1) {min=p0; max=p1;} else {min=p1; max=p0;} \
		rad = fa * boxhalfsize[Y] + fb * boxhalfsize[Z];   \
		if(min>rad || max<-rad) return 0;
	 */
	private static Integer AXISTEST_X2(double a, double b, double fa, double fb, ReferenceObject ro, double[] v0, double[] v1, double[] boxhalfsize) {
		ro.p0 = a*v0[Y] - b*v0[Z];
		ro.p1 = a*v1[Y] - b*v1[Z];
			if(ro.p0<ro.p1) {ro.min=ro.p0; ro.max=ro.p1;} else {ro.min=ro.p1; ro.max=ro.p0;}
		ro.rad = fa * boxhalfsize[Y] + fb * boxhalfsize[Z];
		if(ro.min>ro.rad || ro.max<-ro.rad) return 0;
		return null;
	}

	/**
		#define AXISTEST_Y1(a, b, fa, fb)			   \
		p0 = -a*v0[X] + b*v0[Z];		      	   \
		p1 = -a*v1[X] + b*v1[Z];	     	       	   \
		if(p0<p1) {min=p0; max=p1;} else {min=p1; max=p0;} \
		rad = fa * boxhalfsize[X] + fb * boxhalfsize[Z];   \
		if(min>rad || max<-rad) return 0;
	 */
	private static Integer AXISTEST_Y1(double a, double b, double fa, double fb, ReferenceObject ro, double[] v0, double[] v1, double[] boxhalfsize) {
		ro.p0 = -a*v0[X] + b*v0[Z];
		ro.p1 = -a*v1[X] + b*v1[Z];
		if(ro.p0<ro.p1) {ro.min=ro.p0; ro.max=ro.p1;} else {ro.min=ro.p1; ro.max=ro.p0;}
		ro.rad = fa * boxhalfsize[X] + fb * boxhalfsize[Z];
		if(ro.min>ro.rad || ro.max<-ro.rad) return 0;
		return null;
	}

	/**
		#define FINDMINMAX(x0,x1,x2,min,max) \
		min = max = x0;   \
		if(x1<min) min=x1;\
		if(x1>max) max=x1;\
		if(x2<min) min=x2;\
		if(x2>max) max=x2;
	 */
	private static void FINDMINMAX(double x0, double x1, double x2, ReferenceObject ro) {
		ro.min = x0;
		ro.max = x0;
		if(x1 < ro.min) ro.min = x1;
		if(x1 > ro.max) ro.max = x1;
		if(x2 < ro.min) ro.min = x2;
		if(x2 > ro.max) ro.max = x2;
	}


	/**
	 * math.h function: absolute value of a float (we use a double, which should normally be the fabs() function.
	 */
	private static double fabsf(double x) {
		return Math.abs(x);
	}

	/**
	 * C macro:
	 * <code>#define SUB(dest,v1,v2) \
	 * dest[0]=v1[0]-v2[0]; \
	 * dest[1]=v1[1]-v2[1]; \
	 * dest[2]=v1[2]-v2[2];</code>
	 */
	private static void SUB(double[] dest, double[] v1, double[] v2) {
		dest[0] = v1[0] - v2[0];
		dest[1] = v1[1] - v2[1];
		dest[2] = v1[2] - v2[2];
	}

	/**
		#define CROSS(dest,v1,v2) \
		dest[0]=v1[1]*v2[2]-v1[2]*v2[1]; \
		dest[1]=v1[2]*v2[0]-v1[0]*v2[2]; \
		dest[2]=v1[0]*v2[1]-v1[1]*v2[0];
	*/
	private static void CROSS(double[] dest, double[] v1, double[] v2) {
		dest[0] = v1[1] * v2[2] - v1[2] * v2[1];
		dest[1] = v1[2] * v2[0] - v1[0] * v2[2];
		dest[2] = v1[0] * v2[1] - v1[1] * v2[0];
	}


	/**
	 * Primitive parameters passed by reference in C
	 */
	private static class ReferenceObject {
		public double min, max, p0, p1, p2, rad;
	}
}
